/***
* Feel free to use this code however you wish.
* Created by Alie Tan (www(dot)alietan(dot)com)
*/

#include <mastdlib.h>

#include "PolyTool.h"


float PolyTool::lengthOfLine(b2Vec2 p1, b2Vec2 p2)
{
	float dx, dy;
	dx = p2.x-p1.x;
	dy = p2.y-p1.y;
	return sqrt(dx*dx + dy*dy);
}

bool PolyTool::isPolyConvex(b2Vec2* vertices, int32 vertexCount)
{
	int flag = 0;

	if (vertexCount < 3)
	{
		return false;
	}

	for (int i = 0; i < vertexCount; i++)
	{
		int j = (i + 1) % vertexCount;
		int k = (i + 2) % vertexCount;
		float z  = (vertices[j].x - vertices[i].x) * (vertices[k].y - vertices[j].y);
		z -= (vertices[j].y - vertices[i].y) * (vertices[k].x - vertices[j].x);
		if (z < 0)
		{
			flag |= 1;
		}
		else if (z > 0)
		{
			flag |= 2;
		}
		if (flag == 3)
		{
			return false;
		}
	}
	if (flag != 0)
	{
		return true;
	}
	else
	{
		return false;
	}
}

bool PolyTool::isPolyClockwise(b2Vec2* vertices, int32 vertexCount)
{
	int count = 0;

	if (vertexCount < 3)
	{
		return false;
	}

	for (int i = 0; i < vertexCount; i++)
	{
		int j = (i + 1) % vertexCount;
		int k = (i + 2) % vertexCount;
		float z  = (vertices[j].x - vertices[i].x) * (vertices[k].y - vertices[j].y);
		z -= (vertices[j].y - vertices[i].y) * (vertices[k].x - vertices[j].x);
		if (z < 0)
		{
			count--;
		}
		else if (z > 0)
		{
			count++;
		}
	}
	if (count > 0)
	{
		return false;
	}
	else if (count < 0)
	{
		return true;
	}
	else
	{
		return false;
	}
}

bool PolyTool::insideTriangle(float Ax, float Ay, float Bx, float By, float Cx, float Cy, float Px, float Py)
{
	float ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
	float cCROSSap, bCROSScp, aCROSSbp;

	ax = Cx - Bx;  ay = Cy - By;
	bx = Ax - Cx;  by = Ay - Cy;
	cx = Bx - Ax;  cy = By - Ay;
	apx= Px - Ax;  apy= Py - Ay;
	bpx= Px - Bx;  bpy= Py - By;
	cpx= Px - Cx;  cpy= Py - Cy;

	aCROSSbp = ax*bpy - ay*bpx;
	cCROSSap = cx*apy - cy*apx;
	bCROSScp = bx*cpy - by*cpx;

	return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
}

float PolyTool::area(const b2Vec2 *vertices, int vertexCount)
{
	float A=0.0f;

	for(int p=vertexCount-1,q=0; q<vertexCount; p=q++)
	{
		A+= vertices[p].x*vertices[q].y - vertices[q].x*vertices[p].y;
	}
	return A*0.5f;
}

bool PolyTool::snip(const b2Vec2 *vertices,int u,int v,int w,int n,int *V)
{
	int p;
	float Ax, Ay, Bx, By, Cx, Cy, Px, Py;

	Ax = vertices[V[u]].x;
	Ay = vertices[V[u]].y;

	Bx = vertices[V[v]].x;
	By = vertices[V[v]].y;

	Cx = vertices[V[w]].x;
	Cy = vertices[V[w]].y;

	if ( FLT_EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))) ) return false;

	for (p=0;p<n;p++)
	{
		if( (p == u) || (p == v) || (p == w) )
		{
			continue;
		}
		Px = vertices[V[p]].x;
		Py = vertices[V[p]].y;
		if (insideTriangle(Ax,Ay,Bx,By,Cx,Cy,Px,Py))
		{
			return false;
		}
	}

	return true;
}

int PolyTool::getTriangulatedPoly(const b2Vec2 *vertices, b2Vec2 *result, int vertexCount)
{
	/* allocate and initialize list of Vertices in polygon */
	int myCount = 0;
	if ( vertexCount < 3 )
	{
		return 0;
	}

	int *V = new int[vertexCount];

	/* we want a counter-clockwise polygon in V */
	if ( 0.0f < area(vertices, vertexCount) )
	{
		for (int v=0; v<vertexCount; v++)
		{
			V[v] = v;
		}
	}
	else
	{
		for(int v=0; v<vertexCount; v++)
		{
			V[v] = (vertexCount-1)-v;
		}
	}

	int nv = vertexCount;

	/*  remove nv-2 Vertices, creating 1 triangle every time */
	int count = 2*nv;   /* error detection */

	for(int m=0, v=nv-1; nv>2; )
	{
		/* if we loop, it is probably a non-simple polygon */
		if (0 >= (count--))
		{
			//** Triangulate: ERROR - probable bad polygon!
			return 0;
		}

		/* three consecutive vertices in current polygon, <u,v,w> */
		int u = v  ; if (nv <= u) u = 0;     /* previous */
		v = u+1; if (nv <= v) v = 0;     /* new v    */
		int w = v+1; if (nv <= w) w = 0;     /* next     */

		if ( snip(vertices,u,v,w,nv,V) )
		{
			int a,b,c,s,t;

			/* true names of the vertices */
			a = V[u]; b = V[v]; c = V[w];

			/* output Triangle */
			result[myCount++] = vertices[a];
			result[myCount++] = vertices[b];
			result[myCount++] = vertices[c];

			m++;

			/* remove v from remaining polygon */
			for(s=v,t=v+1;t<nv;s++,t++)
			{
				V[s] = V[t];
			}
			 nv--;

			/* reset error detection counter */
			count = 2*nv;
		}
	}

	delete V;

	return myCount;
}

float PolyTool::isLeft( b2Vec2 p0, b2Vec2 p1, b2Vec2 p2 )
{
    return (p1.x - p0.x)*(p2.y - p0.y) - (p2.x - p0.x)*(p1.y - p0.y);
}

int	PolyTool::getConvexPoly( const b2Vec2* vertices, b2Vec2 *result, int n )
{
    // the output array result[] will be used as the stack
    int    bot=0, top=(-1);  // indices for bottom and top of the stack
    int    i;                // array scan index

    // Get the indices of points with min x-coord and min|max y-coord
    int minmin = 0, minmax;
    float xmin = vertices[0].x;
    for (i=1; i<n; i++)
    {
        if (vertices[i].x != xmin)
		{
        	break;
		}
    }
    minmax = i-1;
    if (minmax == n-1) {       // degenerate case: all x-coords == xmin
        result[++top] = vertices[minmin];
        if (vertices[minmax].y != vertices[minmin].y) // a nontrivial segment
        {
            result[++top] = vertices[minmax];
        }
        result[++top] = vertices[minmin];           // add polygon endpoint
        return top+1;
    }

    // Get the indices of points with max x-coord and min|max y-coord
    int maxmin, maxmax = n-1;
    float xmax = vertices[n-1].x;
    for (i=n-2; i>=0; i--)
    {
        if (vertices[i].x != xmax) break;
    }
    maxmin = i+1;

    // Compute the lower hull on the stack result
    result[++top] = vertices[minmin];      // push minmin point onto stack
    i = minmax;
    while (++i <= maxmin)
    {
        // the lower line joins vertices[minmin] with vertices[maxmin]
        if (isLeft( vertices[minmin], vertices[maxmin], vertices[i]) >= 0 && i < maxmin)
        {
            continue;          // ignore vertices[i] above or on the lower line
        }

        while (top > 0)        // there are at least 2 points on the stack
        {
            // test if vertices[i] is left of the line at the stack top
            if (isLeft( result[top-1], result[top], vertices[i]) > 0)
            {
                break;         // vertices[i] is a new hull vertex
            }
            else
            {
                top--;         // pop top point off stack
            }
        }
        result[++top] = vertices[i];       // push vertices[i] onto stack
    }

    // Next, compute the upper hull on the stack result above the bottom hull
    if (maxmax != maxmin)      // if distinct xmax points
    {
        result[++top] = vertices[maxmax];  // push maxmax point onto stack
    }
    bot = top;                 // the bottom point of the upper hull stack
    i = maxmin;
    while (--i >= minmax)
    {
        // the upper line joins vertices[maxmax] with vertices[minmax]
        if (isLeft( vertices[maxmax], vertices[minmax], vertices[i]) >= 0 && i > minmax)
        {
            continue;          // ignore vertices[i] below or on the upper line
        }

        while (top > bot)    // at least 2 points on the upper stack
        {
            // test if vertices[i] is left of the line at the stack top
            if (isLeft( result[top-1], result[top], vertices[i]) > 0)
            {
                break;         // vertices[i] is a new hull vertex
            }
            else
            {
                top--;         // pop top point off stack
            }
        }
        result[++top] = vertices[i];       // push vertices[i] onto stack
    }
    if (minmax != minmin)
    {
        result[++top] = vertices[minmin];  // push joining endpoint onto stack
    }

    return top+1;
}
